#Copyright (C) Nial Peters 2012
#
#This file is part of AvoScan.
#
#AvoScan is free software: you can redistribute it and/or modify
#it under the terms of the GNU General Public License as published by
#the Free Software Foundation, either version 3 of the License, or
#(at your option) any later version.
#
#AvoScan is distributed in the hope that it will be useful,
#but WITHOUT ANY WARRANTY; without even the implied warranty of
#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#GNU General Public License for more details.
#
#You should have received a copy of the GNU General Public License
#along with AvoScan.  If not, see <http://www.gnu.org/licenses/>.
from Phidgets.PhidgetException import PhidgetException
"""
This module defines the Scanner class which is a wrapper around
the Phidgets.Devices.Stepper class providing a slightly higher 
level interface (for example moving to positions defined in degrees
rather than steps). 
"""

import time
import Queue

from Phidgets.Devices.Stepper import Stepper
from avoscan import ports
from avoscan import compass


class ScannerConnectionTimeoutError(Exception):
    """
    Exception raised if no scanner is found before the connection timeout
    is reached.
    """
    pass


class PhidgetSharedLibNotFoundError(Exception):
    """
    Exception raised if the Phidgets library could not be loaded.
    """
    pass
    
    
class Scanner:
    """
    The Scanner class provides a wrapper around the Phidgets.Devices.Stepper 
    class. It provides a higher level interface to the stepper motor and deals
    with all the setup required to run the motor (setting the speed etc.). 
    
    It is advised that any code that causes the motor to move should use either
    Scanner.set_position() or Scanner.step() to achieve this motion, so that stop
    requests will continue to work as expected.
    """
    def __init__(self):
        #TODO - deg2steps should not be hardcoded here, since it will change for different motors.
        self.__deg2steps = 55.281964025279535 * 16.0 #degrees to steps conversion factor
        self.__engaged = False #is the motor powered up
        self.__stop_flag = False #flag to allow the motor to be stopped mid-move see the stop() method
        self.__cancel_init = False #flag to allow the connection timeout to be cancelled
        self.__stop_queue = Queue.Queue()
        
        #Create a stepper object
        try:
            self.stepper = Stepper()
        except RuntimeError:
            raise PhidgetSharedLibNotFoundError
            
        self.stepper.openPhidget()
        
    
    def set_detach_handler(self, handler_func):
        """
        Register a handler for scanner disconnect events.
        """
        self.stepper.setOnDetachHandler(handler_func)
        
    
    def degrees2steps(self, deg):
        """
        Converts a number of degrees into a number of steps for the motor.
        """
        return deg * self.__deg2steps
    
    
    def get_tracking_speed(self):
        """
        Returns the tracking speed of the motor in degrees per second. Tracking speed
        is the speed used for manual control of the stepper (and is also the maximum 
        speed allowed for scans)
        """
        return 6200.0 / self.__deg2steps
    
    
    def initialise(self, timeout):
        """
        Initialises the Scanner. This must be called before you try to use the 
        Scanner object.
        
        The timeout argument is the number of milliseconds to wait for the scanner 
        to be attached before raising an exception.
        """
        time_waited = 0
        
        while time_waited <= timeout and not self.__cancel_init:
            if self.stepper.isAttached():
                break
            time.sleep(0.5)
            time_waited += 500
        
        if self.__cancel_init:
            #someone impatient didn't want to wait for the stepper to connect - exit now.
            self.close()
            return
        
        if time_waited > timeout:
            #we have exceeded the timeout - raise an error and exit
            raise ScannerConnectionTimeoutError
            
        #otherwise everything went well and we should set up the motor for tracking
        time.sleep(1)
        self.set_to_tracking_speed()
        
        try:
            self.compass = compass.Compass(self)
        except ports.HardwareUnavailableError:
            self.compass = None
    
    
    def cancel_initialisation(self):
        """
        Stops the initialise method running - allows the wait for attachment to be
        cancelled.
        """
        self.__cancel_init = True
           
    
    def get_position(self):
        """
        Returns current position of scanner in degrees. Note that this will depend
        on where the zenith position has been set to by the user.
        """  
        #remember that 90 degrees == 0 steps.  
        return (self.stepper.getCurrentPosition(0) + (90.0 * self.__deg2steps)) / self.__deg2steps
    
    
    def get_serial_num(self):
        """
        Returns the serial number of the stepper controller (this is used as the serial
        number of the AvoScanner).
        """
        return self.stepper.getSerialNum()
    
    
    def get_current(self):
        """
        Returns the current in Amps being drawn by the stepper motor.
        """
        return self.stepper.getCurrent(0)
    
    
    def get_velocity(self):
        """
        Returns the speed that the stepper motor is set to (not necessarily the 
        speed it is currently moving at)
        """
        return self.stepper.getVelocity(0)
    
    def set_position(self, angle):
        """
        Move the stepper motor to the position specified by the angle argument.
        Note that the motor has no idea where 0 degrees is and relies on the 
        user to calibrate it first - see the set_zenith_as_here() method.
        
        Returns True if the requested position was reached or False if not 
        (e.g. if a stop request was received while the motor was moving).
        """
        if not self.__engaged:
            raise Exception, "Stepper is not engaged"
        
        #remember that zenith is set to stepper position 0.
        position = int(round((angle * self.__deg2steps) - (90.0 * self.__deg2steps))) 
        self.stepper.setTargetPosition(0, position)
        
        reached_position_func = lambda x: self.__stop_queue.put(None)

        self.stepper.setOnVelocityChangeHandler(reached_position_func)

        #wait for the motor to move to the desired position
        while not self.__stop_flag:
            if self.stepper.getCurrentPosition(0) == position:
                break
            self.__stop_queue.get()

        self.stepper.setOnVelocityChangeHandler(None)
        
        if self.__stop_flag:
            #then we received a stop request - so stop the motor immediately - we do
            #this by requesting that the motor moves to the position that it is
            #currently at
            try:
                self.stepper.setTargetPosition(0, self.stepper.getCurrentPosition(0))
            
                while not self.stepper.getStopped(0):
                    time.sleep(0.01)
            except PhidgetException:
                #the stepper might be disconnected
                pass

            return False
        
        return True
    
      
    def stop(self):
        """
        Stop the motor as soon as possible, regardless of whether it has got to the position
        that we last requested or not. This method will return as soon as it has managed
        to stop the motor. This may not be immediately.
        """       
        self.__stop_flag = True
        self.__stop_queue.put(None)
        try:
            while not self.stepper.getStopped(0):
                time.sleep(0.05)
        except PhidgetException:
            #the stepper might be disconnected
            pass
        
                 
        self.__stop_flag = False        
                
    
    def step(self, amount):
        """
        Move the scanner by 'amount' degrees (can be negative).
        """
        if not self.__engaged:
            raise Exception, "Stepper is not engaged"
        
        current_angle = self.stepper.getCurrentPosition(0) / self.__deg2steps + 90.0
        self.set_position(current_angle + amount)

    
    def is_engaged(self):
        """
        Returns True if the motor is powered up (engaged), False otherwise.
        """       
        return self.__engaged


    def engage(self):
        """
        Power up the motor. Must be called before set_position() or step() are
        called.
        """
        self.stepper.setEngaged(0, True)
        self.__engaged = True

       
    def disengage(self):
        """
        Power down the motor.
        """
        self.stepper.setEngaged(0, False)
        self.__engaged = False        

            
    def set_zenith_as_here(self):
        """
        Sets the current position of the motor as 90 degrees. This
        will affect subsequent calls to get/set_position.
        """
        self.stepper.setCurrentPosition(0, 0)
   
    
    def close(self):
        """
        Powers down the motor and disconnects from the stepper motor controller.
        The Scanner object becomes unusable after calling this method.
        """
        if self.__engaged:
            self.disengage()
            time.sleep(1)
        try:
            self.stepper.closePhidget()
        except:
            #ignore anything that goes wrong here - could be that the phidget has
            #come unplugged etc.
            pass
  
       
    def set_velocity(self, deg_per_min):
        """
        Set the rotation speed of the motor in degrees per minute.
        """ 
        steps_per_sec = (deg_per_min / 60.0) * self.__deg2steps        
        self.stepper.setVelocityLimit(0, steps_per_sec)
    
       
    def set_to_tracking_speed(self):
        """
        Set the motor speed and acceleration to sensible values for moving it around
        manually e.g. for setting the zenith.
        """ 
        self.set_velocity(self.get_tracking_speed() * 60.0)       
        self.stepper.setAcceleration(0, 87000)  
        self.stepper.setCurrentLimit(0, 0.26) 
        
        
